﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Aldebaran.Proxies;

namespace NAO_Image_Processing
{
    public class NAOControl
    {
        /*statische Variablen*/
        private static double camYAngleOffset = 1.2;
        /*Kontrollvariablen*/
        public bool DEBUG = false;
        public bool DEBUG_PICTURE_MAKER = true;
        string _naoIP;
        int _naoPort;
        bool _naoConnected = false;
        /*Kamera Variablen*/
        double camAngle, camAngleWorld, camHeigth, camXOffset;
        
        /*Proxies definieren*/
        public TextToSpeechProxy _ttsProxy;
        public MotionProxy _motionProxy;
        public RobotPostureProxy _robotPostureProxy;
        public BatteryProxy _batteryProxy;
        public MemoryProxy _memoryProxy;

        public NAOControl(string ip = "127.0.0.1", int port = 9559)
        {
            _naoIP = ip;
            _naoPort = port;
            _naoConnected = true;
            try
            {
                _ttsProxy = new TextToSpeechProxy(_naoIP, _naoPort);
                _motionProxy = new MotionProxy(_naoIP, _naoPort);
                _memoryProxy = new MemoryProxy(_naoIP, _naoPort);
                if (!DEBUG)
                {
                    _batteryProxy = new BatteryProxy(_naoIP, _naoPort);
                    _robotPostureProxy = new RobotPostureProxy(_naoIP, _naoPort);
                }
            }
            catch (Exception ex)
            {
                Console.Error.WriteLine("Error, creating NAO-Proxies: " + ex.Message);
            }
        }

        /*           /| b_1 
         *          / |  
         *         /al|
         *        /   | 
         *       /    |
         *      /-----| b_2
         *     /|\    |
         * c  /a| \   | 
         *   /  |  \ga| 
         *  /   |   \ |
         * /be  | ga2\|
         * -----------------
         * a_c  a_x
         * b=motionProxy.getPosition("CameraTop", 1, true)[2]*100 --> Höhe des Kopfes über dem Boden in cm
         * a=b*tan(alpha)                               Z
         * c=sqrt(a^2+b^2)                              |   
         * ################                             |    
         * Kameraversatz in Y-Richtung = 1.2°          / \
         * Horizontaler Blickwinkel = 60.9°           /   \
         * Vertikaler Blickwinkel = 47.64°           /     \
         * Head-Yaw [Z] = 239° [-119.5° - 119.5°]   X       Y
         * Head-Pitch [X] = 68° [-38.5° - 29.5°]
         */
        public List<double> calcKinematics()
        {
            /* Hardware-Variablen */
            camAngle = (Convert.ToDouble(_memoryProxy.getData("Device/SubDeviceList/HeadPitch/Position/Actuator/Value")) * 180 / Math.PI);
            camAngleWorld = 90 - camYAngleOffset - camAngle;
            /* Kamera Position ermitteln 
             * [0] = X; [1] = Y; [2] = Z
             */
            List<float> camPosition = _motionProxy.getPosition("CameraTop", 1, true);
            
            /* Winkel berechnen */
            double alpha = camAngleWorld;
            double beta = 90 - alpha; //

            /* Abstände bestimmen */
            camXOffset = camPosition[0];
            camHeigth = camPosition[2]; //Höhe der Kamera
            double a = camHeigth * Math.Tan(alpha * Math.PI / 180) - camXOffset; //Abstand Fuß/Boden
            double c = Math.Sqrt(Math.Pow(a, 2) + Math.Pow(camHeigth, 2)); //Abstand Kamera/Boden
            
            /* Werte runden und zurückgeben */
            List<double> retKinematic = new List<double>();
            retKinematic.Add(Math.Round(a * 100, 2)); retKinematic.Add(Math.Round(camHeigth * 100, 2)); retKinematic.Add(Math.Round(c * 100, 2));
            return retKinematic;
        }

        public List<List<double>> getDistances()
        {
            List<List<double>> returnValues = new List<List<double>>();
            List<double> returnAngles = new List<double>();
            List<double> returnDistancesC = new List<double>();
            List<double> returnDistancesA = new List<double>();
            double lowerAngle = 23.82;
            for (double i = camAngleWorld; i > camAngleWorld-lowerAngle; i -= 2.5)
            {
                double a = camHeigth * Math.Tan(i * Math.PI / 180) - camXOffset;
                double c = Math.Sqrt(Math.Pow(a, 2) + Math.Pow(camHeigth, 2));
                returnDistancesC.Add(c);
                returnDistancesA.Add(a);
                returnAngles.Add(i);
            }
            returnValues.Add(returnDistancesC); returnValues.Add(returnAngles); returnValues.Add(returnDistancesA);
            return returnValues;
        }

        public void setConnectedStatus(bool status)
        {
            _naoConnected = status;
        }

        public bool isConnected()
        {
            return _naoConnected;
        }

        public string getIP()
        {
            return _naoIP;
        }

        public int getPort()
        {
            return _naoPort;
        }

        public double getCamAngle()
        {
            return camAngle;
        }

        public bool isTtsAvailable()
        {
            if (_ttsProxy != null)
            {
                return true;
            }
            else
            {
                return false;
            }
        }

        public bool isRobotPostureAvailable()
        {
            if (_robotPostureProxy != null)
            {
                return true;
            }
            else
            {
                return false;
            }
        }

        public bool isMotionAvailable()
        {
            if (_motionProxy != null)
            {
                return true;
            }
            else
            {
                return false;
            }
        }
        
        public bool isBatteryAvailable()
        {
            if (_batteryProxy != null)
            {
                return true;
            }
            else
            {
                return false;
            }
        }

        public bool isMemoryAvailable()
        {
            if (_memoryProxy != null)
            {
                return true;
            }
            else
            {
                return false;
            }
        }

        ~NAOControl()
        {
            try
            {
                _ttsProxy = null;
                _motionProxy = null;
                _robotPostureProxy = null;
                _batteryProxy = null;
                _memoryProxy = null;
            }
            catch (Exception e)
            {
                Console.WriteLine("Exception caught, while disposing Proxies: " + e.Message);
            }
        }
    }
}
